#include <stdio.h>
#include <string.h>
#include "Multiband.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

// 初始化6频段均衡器
void init_6band_equalizer(realtime_multiband_t *processor) {
    memset(processor, 0, sizeof(realtime_multiband_t));
    
    // 频段0: 0-10Hz 低通滤波器 (Butterworth, 2阶, 截止频率10Hz)
    processor->bands[0].b0 = 0.0028982f;
    processor->bands[0].b1 = 0.0057964f;
    processor->bands[0].b2 = 0.0028982f;
    processor->bands[0].a1 = -1.8226949f;
    processor->bands[0].a2 = 0.8371817f;
    processor->bands[0].gain = 1.2f;  // 适度增强惯性感
    processor->bands[0].w0 = 0.0f;
    processor->bands[0].w1 = 0.0f;
    
    // 频段1: 10-15Hz 带通滤波器 (中心频率12.5Hz, 带宽5Hz, Q=2.5)
    processor->bands[1].b0 = 0.0242275f;
    processor->bands[1].b1 = 0.0f;
    processor->bands[1].b2 = -0.0242275f;
    processor->bands[1].a1 = -1.8883405f;
    processor->bands[1].a2 = 0.9110653f;
    processor->bands[1].gain = 1.0f;  // 标准增益
    processor->bands[1].w0 = 0.0f;
    processor->bands[1].w1 = 0.0f;
    
    // 频段2: 15-25Hz 带通滤波器 (中心频率20Hz, 带宽10Hz, Q=2.0)
    processor->bands[2].b0 = 0.0402039f;
    processor->bands[2].b1 = 0.0f;
    processor->bands[2].b2 = -0.0402039f;
    processor->bands[2].a1 = -1.7474471f;
    processor->bands[2].a2 = 0.8772194f;
    processor->bands[2].gain = 1.3f;  // 增强车身动态感
    processor->bands[2].w0 = 0.0f;
    processor->bands[2].w1 = 0.0f;
    
    // 频段3: 25-40Hz 带通滤波器 (中心频率32.5Hz, 带宽15Hz, Q=2.17)
    processor->bands[3].b0 = 0.0476239f;
    processor->bands[3].b1 = 0.0f;
    processor->bands[3].b2 = -0.0476239f;
    processor->bands[3].a1 = -1.5057234f;
    processor->bands[3].a2 = 0.8313738f;
    processor->bands[3].gain = 1.5f;  // 强烈路面纹理
    processor->bands[3].w0 = 0.0f;
    processor->bands[3].w1 = 0.0f;
    
    // 频段4: 40-100Hz 带通滤波器 (中心频率70Hz, 带宽60Hz, Q=1.17)
    processor->bands[4].b0 = 0.0626490f;
    processor->bands[4].b1 = 0.0f;
    processor->bands[4].b2 = -0.0626490f;
    processor->bands[4].a1 = -0.6187808f;
    processor->bands[4].a2 = 0.6128946f;
    processor->bands[4].gain = 1.8f;  // 极致细节增强
    processor->bands[4].w0 = 0.0f;
    processor->bands[4].w1 = 0.0f;
    
    // 频段5: 100Hz+ 高通滤波器 (Butterworth, 2阶, 截止频率100Hz)
    processor->bands[5].b0 = 0.8008024f;
    processor->bands[5].b1 = -1.6016048f;
    processor->bands[5].b2 = 0.8008024f;
    processor->bands[5].a1 = -1.5610181f;
    processor->bands[5].a2 = 0.6413515f;
    processor->bands[5].gain = 0.3f;  // 抑制高频噪声
    processor->bands[5].w0 = 0.0f;
    processor->bands[5].w1 = 0.0f;
    
    processor->sample_count = 0;
    //processor->current_preset = PRESET_BALANCED;
}

// 极速处理函数 - 手动展开循环优化
inline float process_6band_optimized(realtime_multiband_t *p, float x) {
    float y = 0.0f;
    
    // 频段0: 0-10Hz 低通
    {
        biquad_filter_t *f = &p->bands[0];
        float w = x - f->a1 * f->w0 - f->a2 * f->w1;
        float band_out = f->b0 * w + f->b1 * f->w0 + f->b2 * f->w1;
        y += band_out * f->gain;
        f->w1 = f->w0; 
        f->w0 = w;
    }
    
    // 频段1: 10-15Hz 带通
    {
        biquad_filter_t *f = &p->bands[1];
        float w = x - f->a1 * f->w0 - f->a2 * f->w1;
        float band_out = f->b0 * w + f->b1 * f->w0 + f->b2 * f->w1;
        y += band_out * f->gain;
        f->w1 = f->w0; 
        f->w0 = w;
    }
    
    // 频段2: 15-25Hz 带通
    {
        biquad_filter_t *f = &p->bands[2];
        float w = x - f->a1 * f->w0 - f->a2 * f->w1;
        float band_out = f->b0 * w + f->b1 * f->w0 + f->b2 * f->w1;
        y += band_out * f->gain;
        f->w1 = f->w0; 
        f->w0 = w;
    }
    
    // 频段3: 25-40Hz 带通
    {
        biquad_filter_t *f = &p->bands[3];
        float w = x - f->a1 * f->w0 - f->a2 * f->w1;
        float band_out = f->b0 * w + f->b1 * f->w0 + f->b2 * f->w1;
        y += band_out * f->gain;
        f->w1 = f->w0; 
        f->w0 = w;
    }
    
    // 频段4: 40-100Hz 带通
    {
        biquad_filter_t *f = &p->bands[4];
        float w = x - f->a1 * f->w0 - f->a2 * f->w1;
        float band_out = f->b0 * w + f->b1 * f->w0 + f->b2 * f->w1;
        y += band_out * f->gain;
        f->w1 = f->w0; 
        f->w0 = w;
    }
    
    // 频段5: 100Hz+ 高通
    {
        biquad_filter_t *f = &p->bands[5];
        float w = x - f->a1 * f->w0 - f->a2 * f->w1;
        float band_out = f->b0 * w + f->b1 * f->w0 + f->b2 * f->w1;
        y += band_out * f->gain;
        f->w1 = f->w0; 
        f->w0 = w;
    }
    
    p->sample_count++;
    return y;
}

// 设置单个频段增益
void set_band_gain(realtime_multiband_t *processor, FrequencyBand band, float gain) {
    if (band < NUM_BANDS) {
        processor->bands[band].gain = gain;
    }
}

// // 应用预设模式
// void apply_eq_preset(realtime_multiband_t *processor, EQPreset preset) {
//     switch(preset) {
//         case PRESET_BALANCED:
//             // 平衡：自然的路面反馈
//             set_band_gain(processor, BAND_SUBSONIC, 1.2f);
//             set_band_gain(processor, BAND_SUSPENSION, 1.0f);
//             set_band_gain(processor, BAND_BODY_ROLL, 1.3f);
//             set_band_gain(processor, BAND_ROAD_TEXTURE, 1.5f);
//             set_band_gain(processor, BAND_SURFACE_DETAIL, 1.8f);
//             set_band_gain(processor, BAND_ULTRASONIC, 0.3f);
//             break;
            
//         case PRESET_DETAIL:
//             // 细节增强：突出路面信息
//             set_band_gain(processor, BAND_SUBSONIC, 0.8f);
//             set_band_gain(processor, BAND_SUSPENSION, 0.9f);
//             set_band_gain(processor, BAND_BODY_ROLL, 1.1f);
//             set_band_gain(processor, BAND_ROAD_TEXTURE, 2.0f);
//             set_band_gain(processor, BAND_SURFACE_DETAIL, 2.5f);
//             set_band_gain(processor, BAND_ULTRASONIC, 0.5f);
//             break;
            
//         case PRESET_COMFORT:
//             // 舒适：平滑体验
//             set_band_gain(processor, BAND_SUBSONIC, 0.6f);
//             set_band_gain(processor, BAND_SUSPENSION, 0.7f);
//             set_band_gain(processor, BAND_BODY_ROLL, 0.8f);
//             set_band_gain(processor, BAND_ROAD_TEXTURE, 1.0f);
//             set_band_gain(processor, BAND_SURFACE_DETAIL, 1.2f);
//             set_band_gain(processor, BAND_ULTRASONIC, 0.1f);
//             break;
            
//         case PRESET_RACE:
//             // 竞速：强烈反馈
//             set_band_gain(processor, BAND_SUBSONIC, 1.5f);
//             set_band_gain(processor, BAND_SUSPENSION, 1.2f);
//             set_band_gain(processor, BAND_BODY_ROLL, 1.6f);
//             set_band_gain(processor, BAND_ROAD_TEXTURE, 1.8f);
//             set_band_gain(processor, BAND_SURFACE_DETAIL, 2.2f);
//             set_band_gain(processor, BAND_ULTRASONIC, 0.4f);
//             break;
//     }
    
//     processor->current_preset = preset;
// }

// // 获取频段描述
// const char* get_band_description(FrequencyBand band) {
//     switch(band) {
//         case BAND_SUBSONIC:
//             return "0-10Hz: 车辆惯性、持续阻力、沉重感";
//         case BAND_SUSPENSION:
//             return "10-15Hz: 悬挂系统运动、车身俯仰";
//         case BAND_BODY_ROLL:
//             return "15-25Hz: 车身侧倾、中等颠簸";
//         case BAND_ROAD_TEXTURE:
//             return "25-40Hz: 路面粗糙度、柏油纹理";
//         case BAND_SURFACE_DETAIL:
//             return "40-100Hz: 细小颠簸、路面接缝、路肩";
//         case BAND_ULTRASONIC:
//             return "100Hz+: 电机噪声、高频谐振";
//         default:
//             return "Unknown band";
//     }
// }

// 打印均衡器状态
// void print_eq_status(realtime_multiband_t *processor) {
//     ESP_LOGI("EQ", "=== 6-Band Equalizer Status ===");
//     ESP_LOGI("EQ", "Total samples processed: %lu", processor->sample_count);
    
//     const char* preset_names[] = {"BALANCED", "DETAIL", "COMFORT", "RACE"};
//     ESP_LOGI("EQ", "Current preset: %s", preset_names[processor->current_preset]);
    
//     for (int i = 0; i < NUM_BANDS; i++) {
//         ESP_LOGI("EQ", "Band %d: %s", i, get_band_description(i));
//         ESP_LOGI("EQ", "  Gain: %.2f", processor->bands[i].gain);
//     }
// }

// 自适应均衡控制（根据车速）
void adaptive_eq_control(realtime_multiband_t *processor, float vehicle_speed) {
    // 车速越高，低频应该越弱（减少疲劳）
    float speed_factor = 1.0f / (1.0f + vehicle_speed * 0.02f);
    
    // 动态调整低频增益
    processor->bands[BAND_SUBSONIC].gain *= speed_factor;
    processor->bands[BAND_SUSPENSION].gain *= speed_factor;
    
    // 中高频在高速时稍微增强（保持路面感）
    float high_freq_boost = 1.0f + vehicle_speed * 0.01f;
    processor->bands[BAND_ROAD_TEXTURE].gain *= high_freq_boost;
    processor->bands[BAND_SURFACE_DETAIL].gain *= high_freq_boost;
}